//------------------------------------------------------------------------------
//  <copyright file="ParticleFilterLocalizationAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Robotics.Navigation.Runtime.Localization
{
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using System.IO;
    using System.Runtime.Serialization;
    using System.Xml;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Navigation.Runtime.Perception;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// Base class for any SLAM and Localization agents that use Particle Filters.
    /// </summary>
    /// <typeparam name="TFilter">The type of particle filter to use</typeparam>
    /// <typeparam name="TParticleState">The type of particle used by the particle filter</typeparam>
    /// <typeparam name="TObservation">The observation, which must be a kind of horizontal depth profile (kinect or laser).</typeparam>
    /// <typeparam name="TMessage">The type of message generated by this agent</typeparam>
    /// <typeparam name="TDriveFeedbackState">The drive feedback state type used for motion update</typeparam>
    [DataContract]
    public abstract class ParticleFilterLocalizationAgent<TFilter, TParticleState, TObservation, TMessage, TDriveFeedbackState> :
        SynchronizedConsumerProducerAgent<SimpleAgentMessage<TObservation>, SimpleAgentMessage<TDriveFeedbackState>, TMessage>
        where TFilter : ParticleFilter<TParticleState, TDriveFeedbackState, TObservation>
        where TMessage : AgentMessage
        where TObservation : IHorizontalDepthProfile
        where TDriveFeedbackState : IDriveState
    {
        /// <summary>
        /// The default count of particles
        /// </summary>
        private const int DefaultParticleCount = 500;

        /// <summary>
        /// The default message synchronization tolerance, in ms.
        /// </summary>
        private const int DefaultSyncTolerance = 20;

        /// <summary>
        /// Robot configuration folder which contains camera calibration information.
        /// </summary>
        [DataMember(Name = "RobotConfigFolder")]
        private readonly string robotConfigFolder;

        /// <summary>
        /// The name of the agent we subscribe to.
        /// </summary>
        [DataMember(Name = "DriveFeedbackProducer")]
        private readonly string driveFeedbackProducer;

        /// <summary>
        /// The name of the agent we subscribe to.
        /// </summary>
        [DataMember(Name = "LocalMapProducer")]
        private readonly string localMapProducer;

        /// <summary>
        /// The number of particles for the particle filter.
        /// </summary>
        [DataMember(Name = "ParticleCount")]
        private int particleCount;

        /// <summary>
        /// The actual distance resolution
        /// </summary>
        [DataMember(Name = "DistanceResolution")]
        private float distanceResolution;

        /// <summary>
        /// The path to the map file to create/use/update.
        /// </summary>
        [DataMember(Name = "MapFile")]
        private string mapFile;

        /// <summary>
        /// The start pose of the robot.
        /// </summary>
        [DataMember(Name = "RobotStartPose")]
        private Pose2D robotStartPose;

        /// <summary>
        /// The ambiguity in the robot start pose.
        /// </summary>
        [DataMember(Name = "RobotStartPoseVariance")]
        private Pose2D robotStartPoseVariance;

        /// <summary>
        /// The SLAM particle filter.
        /// </summary>
        private TFilter filter;

        /// <summary>
        /// Initializes a new instance of the ParticleFilterLocalizationAgent class
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="localMapProducer">Producer to subscribe to</param>
        /// <param name="driveFeedbackProducer">Drive feedback producer to subscribe to</param>
        /// <param name="robotConfigFolder">The robot configuration folder.</param>
        /// <param name="distanceResolution">The actual distance resolution.</param>
        /// <param name="particleCount">The number of particles for the particle filter.</param>
        /// <param name="mapFile">The map to load.</param>
        /// <param name="robotStartPose">The starting pose of the robot, if known</param>
        /// <param name="robotStartPoseVariance">The starting pose variance of the robot</param>
        public ParticleFilterLocalizationAgent(
            string name,
            IProducer<SimpleAgentMessage<TObservation>> localMapProducer,
            IProducer<SimpleAgentMessage<TDriveFeedbackState>> driveFeedbackProducer,
            string robotConfigFolder,
            float distanceResolution,
            int particleCount,
            string mapFile,
            Pose2D robotStartPose,
            Pose2D robotStartPoseVariance)
            : base(name, DefaultSyncTolerance)
        {
            this.localMapProducer = localMapProducer.Name;
            this.driveFeedbackProducer = driveFeedbackProducer.Name;
            this.robotConfigFolder = robotConfigFolder;
            this.distanceResolution = distanceResolution;
            this.particleCount = particleCount;
            this.mapFile = mapFile;
            this.robotStartPose = robotStartPose;
            this.robotStartPoseVariance = robotStartPoseVariance;
        }

        /// <summary>
        /// Gets the name of the agent that produces local maps.
        /// </summary>
        protected override string ProducerName1
        {
            get { return this.localMapProducer; }
        }

        /// <summary>
        /// Gets the name of the agent that generates odometry info.
        /// </summary>
        protected override string ProducerName2
        {
            get { return this.driveFeedbackProducer; }
        }

        /// <summary>
        /// Gets the number of particles for the particle filter.
        /// </summary>
        protected int ParticleCount
        {
            get { return this.particleCount; }
        }

        /// <summary>
        /// Gets the SLAM particle filter.
        /// </summary>
        protected TFilter Filter
        {
            get { return this.filter; }
        }

        /// <summary>
        /// Gets the location of the map file.
        /// </summary>
        protected string MapFile
        {
            get { return this.mapFile; }
        }

        /// <summary>
        /// Gets or sets the starting pose of the robot, if known.
        /// </summary>
        protected Pose2D RobotStartPose
        {
            get { return this.robotStartPose; }
            set { this.robotStartPose = value; }
        }
        
        /// <summary>
        /// Gets the starting pose of the robot, if known.
        /// </summary>
        protected Pose2D RobotStartPoseVariance
        {
            get { return this.robotStartPoseVariance; }
        }

        /// <summary>
        /// Gets the distance resolution of the map.
        /// </summary>
        protected double DistanceResolution
        {
            get { return this.distanceResolution; }
        }

        /// <summary>
        /// Reset the filter with a new start pose
        /// </summary>
        /// <param name="map">The global map</param>
        /// <param name="distanceResolution">The resolution of the global map</param>
        /// <param name="robotStartPose">The robot start pose</param>
        public void ResetFilter(GlobalMap map, float distanceResolution, Pose2D robotStartPose)
        {
            this.filter = this.CreateFilter(map, this.distanceResolution, robotStartPose);
        }

        /// <summary>
        /// Initialize the agent
        /// </summary>
        /// <param name="locator">The agent locator</param>
        public override void Initialize(AgentLocator locator)
        {
            if (this.distanceResolution == 0)
            {
                this.distanceResolution = GlobalMap.DefaultDistanceResolution;
            }

            if (this.particleCount == 0)
            {
                this.particleCount = DefaultParticleCount;
            }

            base.Initialize(locator);
        }

        /// <summary>
        /// Called when the agent transitions from inactive to active.
        /// </summary>
        public override void OnActivated()
        {
            // If we start off w/o a map - then the map origin is the same as robot start pose, otherwise, we'll load the map origin pose from the map file
            this.filter = null;

            GlobalMap map = null;
            if (this.mapFile != null && File.Exists(this.mapFile))
            {
                map = Serializer.Load<GlobalMap>(this.mapFile);
                Console.WriteLine("Loaded map file: {0}, ({1})", this.mapFile, File.GetLastWriteTime(this.mapFile));
                this.distanceResolution = map.DistanceResolution;
            }

            this.filter = this.CreateFilter(map, this.distanceResolution, this.robotStartPose);

            base.OnActivated();
        }

        /// <summary>
        /// Processes a synced pair of local map and odometry info. 
        /// It invokes the particle filter to update the particles based on the observation (local map) and control update (odometry).
        /// </summary>
        /// <param name="observationMessage">The synchronized local map message</param>
        /// <param name="driveFeedbackMessage">The synchronized odometry message</param>
        protected override void Receive(SimpleAgentMessage<TObservation> observationMessage, SimpleAgentMessage<TDriveFeedbackState> driveFeedbackMessage)
        {
                this.filter.Update(driveFeedbackMessage.State, observationMessage.State);
                TMessage newMessage = this.CreateMessage(observationMessage);
                this.Publisher.Post(newMessage);
            }

        /// <summary>
        /// Creates a new filter. Called on activation of the agent or when resetting the filter (as in click-to-localize).
        /// </summary>
        /// <param name="map">The map to start with.</param>
        /// <param name="distanceResolution">The resolution of the map</param>
        /// <param name="robotStartPose">The starting pose</param>
        /// <returns>A filter</returns>
        protected abstract TFilter CreateFilter(GlobalMap map, float distanceResolution, Pose2D robotStartPose);

        /// <summary>
        /// Creates a new message to publish, based on the agent state. Called every time the filter is updated.
        /// </summary>
        /// <param name="initiatingMessage">
        /// The lower-level message that triggered the creation of this message.
        /// </param>
        /// <returns>The message to publish</returns>
        protected abstract TMessage CreateMessage(SimpleAgentMessage<TObservation> initiatingMessage);

        /// <summary>
        /// Transforms a map-relative pose (pixels relative to start pose on the map) to a global pose (meters relative to the start pose of the robot).
        /// </summary>
        /// <param name="mapPose">The pose in map units</param>
        /// <param name="mapOriginPose">The origin of the map</param>
        /// <returns>The pose in global units</returns>
        protected Pose2D GetPoseInGlobalCoordinates(Pose2D mapPose, Pose2D mapOriginPose)
        {
            double x = (mapPose.X - mapOriginPose.X) * this.distanceResolution;
            double y = (mapPose.Y - mapOriginPose.Y) * this.distanceResolution;
            double h = mapPose.Heading - mapOriginPose.Heading;
            Pose2D pose = new Pose2D(x, y, h);
            return pose;
        }
    }
}
